#include "RadianEditorMathHelper.h"

#include "OgreEntity.h"
#include "OgreMesh.h"
#include "OgreCamera.h"
#include "OgreSceneManager.h"
#include "OgreManualObject.h"
#include "OgreSceneQuery.h"
#include "OgreSubMesh.h"
#include "OgreSubEntity.h"

//--------------------------------------------------------------------------
bool RadianEditorMathHelper::DoSceneQuery(Ogre::Camera* camera,Ogre::SceneManager* sceneMgr
										  ,const Ogre::Vector2& screenPos,const Ogre::String& movableType
										  , Ogre::MovableObject*& outObj,Math::Vector3& outPos)
{
	Ogre::Ray camRay = camera->getCameraToViewportRay(screenPos.x,screenPos.y);

	Ogre::RaySceneQuery* raySceneQuery = sceneMgr->createRayQuery(camRay);
	raySceneQuery->setSortByDistance(true);

	Ogre::RaySceneQueryResult& queryResult = raySceneQuery->execute();
	Ogre::RaySceneQueryResult::iterator resultIt = queryResult.begin();

	while(resultIt != queryResult.end())
	{
		if(movableType != Ogre::StringUtil::BLANK && resultIt->movable && resultIt->movable->getMovableType() == movableType)
		{
			outObj = resultIt->movable;
			outPos = camRay.getPoint(resultIt->distance);
			delete raySceneQuery;
			return true;
		}

		else if(movableType == Ogre::StringUtil::BLANK && resultIt->movable)
		{
			outObj = resultIt->movable;
			outPos = camRay.getPoint(resultIt->distance);
			delete raySceneQuery;
			return true;
		}

		resultIt++;
	}

	return false;
}
//--------------------------------------------------------------------------
void RadianEditorMathHelper::PlaceOnMesh(Ogre::Camera* camera,Ogre::SceneManager* sceneMgr,const Ogre::Vector2& screenPos,Ogre::Entity* outEntity,Math::Vector3 outPos)
{
	// TO DO...
}
//--------------------------------------------------------------------------
void RadianEditorMathHelper::GetMeshInformation(Ogre::MeshPtr mesh,size_t &vertex_count,Math::Vector3* &vertices,
						size_t &index_count, unsigned* &indices,
						const Math::Vector3& position,
						const Ogre::Quaternion& orient,const Math::Vector3& scale)
{
	bool added_shared = false;
	size_t current_offset = 0;
	size_t shared_offset = 0;
	size_t next_offset = 0;
	size_t index_offset = 0;

	vertex_count = index_count = 0;

	// Calculate how many vertices and indices we're going to need
	for (unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i)
	{
		Ogre::SubMesh* submesh = mesh->getSubMesh( i );

		// We only need to add the shared vertices once
		if(submesh->useSharedVertices)
		{
			if( !added_shared )
			{
				vertex_count += mesh->sharedVertexData->vertexCount;
				added_shared = true;
			}
		}
		else
		{
			vertex_count += submesh->vertexData->vertexCount;
		}

		// Add the indices
		index_count += submesh->indexData->indexCount;
	}


	// Allocate space for the vertices and indices
	vertices = new Math::Vector3[vertex_count];
	indices = new unsigned int[index_count];

	added_shared = false;

	// Run through the submeshes again, adding the data into the arrays
	for ( unsigned short i = 0; i < mesh->getNumSubMeshes(); ++i)
	{
		Ogre::SubMesh* submesh = mesh->getSubMesh(i);

		Ogre::VertexData* vertex_data = submesh->useSharedVertices ? mesh->sharedVertexData : submesh->vertexData;

		if((!submesh->useSharedVertices)||(submesh->useSharedVertices && !added_shared))
		{
			if(submesh->useSharedVertices)
			{
				added_shared = true;
				shared_offset = current_offset;
			}

			const Ogre::VertexElement* posElem =
				vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);

			Ogre::HardwareVertexBufferSharedPtr vbuf =
				vertex_data->vertexBufferBinding->getBuffer(posElem->getSource());

			unsigned char* vertex =
				static_cast<unsigned char*>(vbuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));

			// There is _no_ baseVertexPointerToElement() which takes an Ogre::Real or a double
			//  as second argument. So make it float, to avoid trouble when Ogre::Real will
			//  be comiled/typedefed as double:
			//      Ogre::Real* pReal;
			float* pReal;

			for( size_t j = 0; j < vertex_data->vertexCount; ++j, vertex += vbuf->getVertexSize())
			{
				posElem->baseVertexPointerToElement(vertex, &pReal);

				Math::Vector3 pt(pReal[0], pReal[1], pReal[2]);

				vertices[current_offset + j] = (orient * (pt * scale)) + position;
			}

			vbuf->unlock();
			next_offset += vertex_data->vertexCount;
		}


		Ogre::IndexData* index_data = submesh->indexData;
		size_t numTris = index_data->indexCount / 3;
		Ogre::HardwareIndexBufferSharedPtr ibuf = index_data->indexBuffer;
		if( ibuf.isNull() ) continue; // need to check if index buffer is valid (which will be not if the mesh doesn't have triangles like a pointcloud)

		bool use32bitindexes = (ibuf->getType() == Ogre::HardwareIndexBuffer::IT_32BIT);

		unsigned long*  pLong = static_cast<unsigned long*>(ibuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
		unsigned short* pShort = reinterpret_cast<unsigned short*>(pLong);


		size_t offset = (submesh->useSharedVertices)? shared_offset : current_offset;
		size_t index_start = index_data->indexStart;
		size_t last_index = numTris*3 + index_start;

		if (use32bitindexes)
			for (size_t k = index_start; k < last_index; ++k)
			{
				indices[index_offset++] = pLong[k] + static_cast<unsigned long>( offset );
			}

		else
			for (size_t k = index_start; k < last_index; ++k)
			{
				indices[ index_offset++ ] = static_cast<unsigned long>( pShort[k] ) +
					static_cast<unsigned long>( offset );
			}

			ibuf->unlock();
			current_offset = next_offset;
	}
	
}
//--------------------------------------------------------------------------
bool RadianEditorMathHelper::GetScreenSpaceCoords(Ogre::MovableObject* object, Ogre::Camera* camera, Ogre::Vector2& result)
{

	if(!object->isInScene())
		return false;
 
	const Math::AxisAlignedBox &AABB = object->getWorldBoundingBox(true);

	/**
	* If you need the point above the object instead of the center point:
	* This snippet derives the average point between the top-most corners of the bounding box
	* Math::Vector3 point = (AABB.getCorner(AxisAlignedBox::FAR_LEFT_TOP)
	*    + AABB.getCorner(AxisAlignedBox::FAR_RIGHT_TOP)
	*    + AABB.getCorner(AxisAlignedBox::NEAR_LEFT_TOP)
	*    + AABB.getCorner(AxisAlignedBox::NEAR_RIGHT_TOP)) / 4;
	*/

	// Get the center point of the object's bounding box
	Math::Vector3 point = AABB.getCenter();

	// Is the camera facing that point? If not, return false
	Ogre::Plane cameraPlane = Ogre::Plane(Math::Vector3(camera->getDerivedOrientation().zAxis()), camera->getDerivedPosition());

	if(cameraPlane.getSide(point) != Ogre::Plane::NEGATIVE_SIDE)
		return false;

	// Transform the 3D point into screen space
	point = camera->getProjectionMatrix() * (camera->getViewMatrix() * point);

	// Transform from coordinate space [-1, 1] to [0, 1] and update in-value
	result.x = (point.x / 2) + 0.5f;
	result.y = 1 - ((point.y / 2) + 0.5f);

	return true;
}
//--------------------------------------------------------------------------
Ogre::Vector2 RadianEditorMathHelper::WorldToScreen2D(const Math::Vector3& worldPos,Ogre::Camera* camera,size_t width,size_t height)
{
	Ogre::Vector4 screenPos = camera->getProjectionMatrix() * camera->getViewMatrix() * Ogre::Vector4(worldPos.x, worldPos.y, worldPos.z, 1);

	float screenX = ( 1 + screenPos.x / screenPos.w) * 0.5f * (float)width;
	float screenY = ( 1 - screenPos.y / screenPos.w) * 0.5f * (float)height;

	return Ogre::Vector2(screenX, screenY);

}
//--------------------------------------------------------------------------
float RadianEditorMathHelper::DistanceToLine(float x1,float y1,float x2, float y2,float pX,float pY)
{
	Ogre::Vector2 ab(x1 - x2, y1 - y2);
	Ogre::Vector2 bc(x2 - pX, y2 - pY);
	Ogre::Vector2 ca(pX - x1, pY - y1);

	if(ab.dotProduct(-ca) < 0 || ab.dotProduct(-bc) < 0)
		return FLT_MAX;	

	float a = ab.length();
	float b = bc.length();
	float c = ca.length();
	float p = (a + b + c) * 0.5F;
	float area = (float)Ogre::Math::Sqrt(p * (p - a) * (p - b) * (p - c));
	return area * 2 / a;
}
//--------------------------------------------------------------------------
Ogre::Vector2 RadianEditorMathHelper::GetScreenPos(const QPointF& qtPos,const QSize& qtSize)
{		
	float posX = qtPos.x() / (float)qtSize.width();
	float posY = qtPos.y() / (float)qtSize.height();
	
	return Ogre::Vector2(posX, posY);
}
//--------------------------------------------------------------------------
Math::AxisAlignedBox RadianEditorMathHelper::GetMergedAABB(const SceneNodeList& sceneNodes)
{
	Math::AxisAlignedBox aabb;
	SceneNodeList::const_iterator sceneNodeIt = sceneNodes.begin();
	while(sceneNodeIt != sceneNodes.end())
	{
		aabb.merge((*sceneNodeIt)->_getWorldAABB());
		++sceneNodeIt;
	}

	return aabb;
}
//--------------------------------------------------------------------------